/*
 *  This code allows to change the P+I+D parameters from BT
 *  By Rolf Ziegler, Feb. 2014
 * 
*/
#include <p18cxxx.h>
#include <usart.h>
#include <stdio.h>
#include <string.h>
#include "BT_command.h"
#include "io_cfg.h"
#include "PID.h"
#include "user.h"
#include "adc_util.h"
#include "EEPROM.h"
#include "robot_helper.h"

void displayCommands(void)
{
        fprintf(_H_USART,"Balancing Robot\r\n\r\n");
        fprintf(_H_USART,"Type any character to start\r\n");
        
        fprintf(_H_USART,"PID setup\r\n");
        fprintf(_H_USART,"K   = +0.1, k=-0.1\r\n");
        fprintf(_H_USART,"P   = +0.1, p=-0.1\r\n");
        fprintf(_H_USART,"I   = +0.1, i=-0.1\r\n");
        fprintf(_H_USART,"D   = +0.1, d=-0.1\r\n");
        fprintf(_H_USART,"8   = forward 2=backward\r\n");
        fprintf(_H_USART,"b/B = battery level\r\n");
        fprintf(_H_USART,"s   = stop, r=run robot\r\n");
        fprintf(_H_USART,"x/X = reset values to default\r\n");
        fprintf(_H_USART,"m/M = store data to EEPROM\r\n");
        fprintf(_H_USART,"      restore with robot reset\r\n");
        fprintf(_H_USART,"-------------------\r\n");
        // problems printing float, so let's use a self made print routine.
        putrsUSART("Kp =");
        printFloat(Kp);
        putrsUSART("Ki =");
        printFloat(Ki);
        putrsUSART("Kd =");
        printFloat(Kd);
        putrsUSART("K  =");
        printFloat(K);
        fprintf(_H_USART,"Balance=%d\r\n",balance);
        fprintf(_H_USART,"-------------------\r\n");
}

char getBTCommand(void)
{
    char command;
    // check if data is ready

    if(DataRdyUSART()==1)
        {
        command=ReadUSART ();
        mLED_2_Toggle();
        
        switch(command)
            {
            case 'p':
                Kp-=0.1;
                putrsUSART("Kp=");
                printFloat(Kp);
                break;
            case 'P':
                Kp+=0.1;
                putrsUSART("Kp=");
                printFloat(Kp);
                break;
            case 'i':
                Ki-=0.1;
                putrsUSART("Ki=");
                printFloat(Ki);
                break;
            case 'I':
                Ki+=0.1;
                putrsUSART("Ki=");
                printFloat(Ki);
                break;
            case 'd':
                Kd-=0.1;
                putrsUSART("Kd=");
                printFloat(Kd);
                break;
            case 'D':
                Kd+=0.1;
                putrsUSART("Kd=");
                printFloat(Kd);
                break;
            case 'k':
                K-=0.1;
                putrsUSART("K=");
                printFloat(K);
                break;
            case 'K':
                K+=0.1;
                putrsUSART("K=");
                printFloat(K);
                break;
            case '8':
                balance++;
                fprintf(_H_USART,"%d\r\n",balance);
                break;
            case '2':
                balance--;
                fprintf(_H_USART,"%d\r\n",balance);
                break;
            case 's':
                fprintf(_H_USART,"robot stopped (r to run)\r\n");
                runRobot=0;
                break;
            case 'r':
                fprintf(_H_USART,"robot run (s to stop)\r\n");
                runRobot=1;
                break;
            case 'b': case 'B':
                //fprintf(_H_USART,"bat: %u\r\n",0x0ff & accuLevel);
                fprintf(_H_USART,"battery level");
                printFloatVolt(ACCU_CALIB * accuLevel);
                break;

            case 'M': case 'm':

                storePID();

                fprintf(_H_USART,"PID parameters stored !\r\n");
                // store_eeprom();
                break;
            case 'x': case 'X':
                K = DEFAULT_K;
                Kp = DEFAULT_KP;
                Ki = DEFAULT_KI;
                Kd = DEFAULT_KD; 
                balance=0;
                fprintf(_H_USART,"PID values reset!\r\n");
                break;

            default:
                displayCommands();
                break;
            }
        }
}